#include "cartographer/mapping/internal/global_trajectory_builder.h"
#include <memory>
#include "absl/memory/memory.h"
#include "absl/types/optional.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/internal/local_slam_result_data.h"
#include "cartographer/mapping/internal/motion_filter.h"
#include "cartographer/metrics/family_factory.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        namespace
        {
            static auto *kLocalSlamMatchingResults = metrics::Counter::Null();
            static auto *kLocalSlamInsertionResults = metrics::Counter::Null();

            // 模板类写在源文件中
            template<typename LocalTrajectoryBuilder, typename PoseGraph>
            class GlobalTrajectoryBuilder : public mapping::TrajectoryBuilderInterface
            {
            public:
                // Passing a 'nullptr' for 'local_trajectory_builder' is acceptable, but no
                // 'TimedPointCloudData' may be added in that case.
                GlobalTrajectoryBuilder(std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder, const int trajectory_id,
                                        PoseGraph *const pose_graph, const LocalSlamResultCallback &local_slam_result_callback,
                                        const absl::optional<MotionFilter> &pose_graph_odometry_motion_filter)
                    : trajectory_id_(trajectory_id)
                    , pose_graph_(pose_graph)
                    , local_trajectory_builder_(std::move(local_trajectory_builder))
                    , local_slam_result_callback_(local_slam_result_callback)
                    , pose_graph_odometry_motion_filter_(pose_graph_odometry_motion_filter)
                {
                }
                ~GlobalTrajectoryBuilder() override
                {
                }

                GlobalTrajectoryBuilder(const GlobalTrajectoryBuilder &) = delete;
                GlobalTrajectoryBuilder &operator=(const GlobalTrajectoryBuilder &) = delete;

                void AddSensorData(const std::string &sensor_id, const sensor::TimedPointCloudData &timed_point_cloud_data) override
                {
                    CHECK(local_trajectory_builder_) << "Cannot add TimedPointCloudData without a LocalTrajectoryBuilder.";
                    // 前端入口
                    std::unique_ptr<typename LocalTrajectoryBuilder::MatchingResult> matchingRes =
                        local_trajectory_builder_->AddRangeData(sensor_id, timed_point_cloud_data);
                    if (matchingRes == nullptr)
                    {
                        // The range data has not been fully accumulated yet.
                        return;
                    }
                    kLocalSlamMatchingResults->Increment();
                    std::unique_ptr<InsertionResult> insertRes;
                    if (matchingRes->insertion_result != nullptr)
                    {
                        auto localInsert = std::move(matchingRes->insertion_result);
                        if (localInsert->constant_data != nullptr)
                        {
                            kLocalSlamInsertionResults->Increment();
                            // 后端入口
                            auto node_id = pose_graph_->AddNode(localInsert->constant_data, trajectory_id_, localInsert->insertion_submaps);
                            CHECK_EQ(node_id.trajectory_id, trajectory_id_);
                            // 这个是TrajectoryBuilderInterface里面定义的InsertionResult
                            insertRes = absl::make_unique<InsertionResult>(
                                InsertionResult{node_id, localInsert->constant_data,
                                                std::vector<std::shared_ptr<const Submap>>(localInsert->insertion_submaps.begin(),
                                                                                           localInsert->insertion_submaps.end()),
                                                localInsert->dynamicCnts});
                        }
                        else if (!localInsert->dynamicCnts.empty())
                        {
                            insertRes =
                                absl::make_unique<InsertionResult>(InsertionResult{NodeId(-1, -1), nullptr, {}, localInsert->dynamicCnts});
                        }
                    }
                    if (local_slam_result_callback_)
                    {
                        // 回传给ROS部分做发布
                        local_slam_result_callback_(trajectory_id_, matchingRes->time, matchingRes->local_pose,
                                                    std::move(matchingRes->range_data_in_local), std::move(insertRes));
                    }
                }

                void AddSensorData(const std::string &sensor_id, const sensor::ImuData &imu_data) override
                {
                    if (local_trajectory_builder_)
                    {
                        local_trajectory_builder_->AddImuData(imu_data);
                    }
                    pose_graph_->AddImuData(trajectory_id_, imu_data);
                }

                // 无明显运动的里程计数据不会加入后端，而会立即加入前端;因为前端要用里程计插值，所以需要实时数据，后端用里程计
                // 参与优化，频率比较低
                void AddSensorData(const std::string &sensor_id, const sensor::OdometryData &odometry_data) override
                {
                    CHECK(odometry_data.pose.IsValid()) << odometry_data.pose;
                    if (local_trajectory_builder_)
                    {
                        local_trajectory_builder_->AddOdometryData(odometry_data);
                    }
                    // TODO(MichaelGrupp): Instead of having an optional filter on this level,
                    // odometry could be marginalized between nodes in the pose graph.
                    // Related issue: cartographer-project/cartographer/#1768
                    if (pose_graph_odometry_motion_filter_.has_value() &&
                        pose_graph_odometry_motion_filter_.value().IsSimilar(odometry_data.time, odometry_data.pose))
                    {
                        return;
                    }
                    pose_graph_->AddOdometryData(trajectory_id_, odometry_data);
                }

                void AddSensorData(const std::string &sensor_id, const sensor::FixedFramePoseData &fixed_frame_pose) override
                {
                    if (fixed_frame_pose.pose.has_value())
                    {
                        CHECK(fixed_frame_pose.pose.value().IsValid()) << fixed_frame_pose.pose.value();
                    }
                    pose_graph_->AddFixedFramePoseData(trajectory_id_, fixed_frame_pose);
                }

                void AddSensorData(const std::string &sensor_id, const sensor::LandmarkData &landmark_data) override
                {
                    pose_graph_->AddLandmarkData(trajectory_id_, landmark_data);
                }

                void AddLocalSlamResultData(std::unique_ptr<mapping::LocalSlamResultData> local_slam_result_data) override
                {
                    CHECK(!local_trajectory_builder_) << "Can't add LocalSlamResultData with "
                                                         "local_trajectory_builder_ present.";
                    local_slam_result_data->AddToPoseGraph(trajectory_id_, pose_graph_);
                }

            private:
                const int trajectory_id_;
                PoseGraph *const pose_graph_;
                std::unique_ptr<LocalTrajectoryBuilder> local_trajectory_builder_;
                LocalSlamResultCallback local_slam_result_callback_;
                absl::optional<MotionFilter> pose_graph_odometry_motion_filter_;
            };

        } // namespace

        std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder2D(
            std::unique_ptr<LocalTrajectoryBuilder2D> local_trajectory_builder, const int trajectory_id,
            mapping::PoseGraph2D *const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback &local_slam_result_callback,
            const absl::optional<MotionFilter> &pose_graph_odometry_motion_filter)
        {
            return absl::make_unique<GlobalTrajectoryBuilder<LocalTrajectoryBuilder2D, mapping::PoseGraph2D>>(
                std::move(local_trajectory_builder), trajectory_id, pose_graph, local_slam_result_callback,
                pose_graph_odometry_motion_filter);
        }

        std::unique_ptr<TrajectoryBuilderInterface> CreateGlobalTrajectoryBuilder3D(
            std::unique_ptr<LocalTrajectoryBuilder3D> local_trajectory_builder, const int trajectory_id,
            mapping::PoseGraph3D *const pose_graph, const TrajectoryBuilderInterface::LocalSlamResultCallback &local_slam_result_callback,
            const absl::optional<MotionFilter> &pose_graph_odometry_motion_filter)
        {
            return absl::make_unique<GlobalTrajectoryBuilder<LocalTrajectoryBuilder3D, mapping::PoseGraph3D>>(
                std::move(local_trajectory_builder), trajectory_id, pose_graph, local_slam_result_callback,
                pose_graph_odometry_motion_filter);
        }

        void GlobalTrajectoryBuilderRegisterMetrics(metrics::FamilyFactory *factory)
        {
            auto *results = factory->NewCounterFamily("mapping_global_trajectory_builder_local_slam_results", "Local SLAM results");
            kLocalSlamMatchingResults = results->Add({{"type", "MatchingResult"}});
            kLocalSlamInsertionResults = results->Add({{"type", "InsertionResult"}});
        }

    } // namespace mapping
} // namespace cartographer
